using System;
using System.Runtime.InteropServices;
using GLWrapper;

namespace GameBaseCode
{

    public class Matrix3
    {
        float[] _m = new float[9];

	public float this[int i]
    {
        get { 
            return _m[i];
        }
        set {
            _m[i] = value;
        }
    }

        public static Matrix3 ang2mat(float hang, float vang)
        {
            Matrix3 mat = new Matrix3();
            float cv = (float)Math.Cos(vang);
            float sv = (float)Math.Sin(vang);
            float ch = (float)Math.Cos(hang);
            float sh = (float)Math.Sin(hang);
            mat[6] = cv*sh; mat[0] = ch;
            mat[7] = sv;    mat[1] = 0;
            mat[8] = cv*ch; mat[2] =-sh;
            mat[3] = mat[7]*mat[2] - mat[8]*mat[1];
            mat[4] = mat[8]*mat[0] - mat[6]*mat[2];
            mat[5] = mat[6]*mat[1] - mat[7]*mat[0];
            return mat;
        }

        public void setIdentity()
        {
            this[0] = 1; this[1] = 0; this[2] = 0;
            this[3] = 0; this[4] = 1; this[5] = 0;
            this[6] = 0; this[7] = 0; this[8] = 1;
        }

        public static Matrix3 initRotationMatrix(Vector3 axis, float r)
        {
            // The axis vector must be of unit length
            float m = axis.Magnitude;
            float x = axis[0] / m;
            float y = axis[1] / m;
            float z = axis[2] / m;

            // Compute the rotation matrix  
            float c = (float)Math.Cos(r); float s = (float)Math.Sin(r);
            Matrix3 mtx = new Matrix3();
            mtx[0] = (x * x) * (1.0f - c) + c;
            mtx[1] = (y * x) * (1.0f - c) + (z * s);
            mtx[2] = (z * x) * (1.0f - c) - (y * s);

            mtx[3] = (x * y) * (1.0f - c) - (z * s);
            mtx[4] = (y * y) * (1.0f - c) + c;
            mtx[5] = (z * y) * (1.0f - c) + (x * s);

            mtx[6] = (x * z) * (1.0f - c) + (y * s);
            mtx[7] = (y * z) * (1.0f - c) - (x * s);
            mtx[8] = (z * z) * (1.0f - c) + c;
            return mtx;
        }
        // rotasjon x:
        // | 1 0  0 |
        // | 0 c -s |
        // | 0 s  c |
     public void rotateX(float a)
      {
	    Matrix3 tmp = new Matrix3();
	    tmp.setIdentity();
    	
        float cosa= (float) Math.Cos(a);
	    float sina=(float) Math.Sin(a);
	    tmp[4]=cosa;
	    tmp[5]=-sina;
	    tmp[7]=sina;
	    tmp[8]=cosa;
	    this.mul(tmp);	
      }
     // rotasjon y:
     // | c 0  s |
     // | 0 1  0 |
     // | -s 0 c |
     public void rotateY(float a)
      {
	    Matrix3 tmp = new Matrix3();
	    tmp.setIdentity();
    	
        float cosa= (float) Math.Cos(a);
	    float sina=(float) Math.Sin(a);
	    tmp[0]=cosa;
	    tmp[2]=sina;
	    tmp[6]=-sina;
	    tmp[8]=cosa;
	    this.mul(tmp);	
      }
     // rotasjon z:
     // | c -s 0 |
     // | s c  0 |
     // | 0 0  1 |
      public void rotateZ(float a)
      {
	    Matrix3 tmp = new Matrix3();
	    tmp.setIdentity();
    	
        float cosa= (float) Math.Cos(a);
	    float sina=(float) Math.Sin(a);
	    tmp[0]=cosa;
	    tmp[1]=-sina;
	    tmp[3]=sina;
	    tmp[4]=cosa;
	    this.mul(tmp);	
      }
  


        public Vector3 transform(Vector3 v)
        {
            return new Vector3(_m[0] * v.x + _m[1] * v.y + _m[2] * v.z,
                               _m[3] * v.x + _m[4] * v.y + _m[5] * v.z,
                               _m[6] * v.x + _m[7] * v.y + _m[8] * v.z);
        }

        public void set(Vector3 left, Vector3 up, Vector3 dir)
        {
            for (int i = 0; i < 3; i++)
            {
                _m[0 + i] = left[i];
                _m[3 + i] = up[i];
                _m[6 + i] = dir[i];
            }
        }

        public static Matrix3 mul(Matrix3 a, Matrix3 b)
        {
            Matrix3 newMat = new Matrix3();
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    newMat[i*3+j] = 0;

            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    for (int k = 0; k < 3; k++)
                        newMat[i*3+j] += a[i*3+k] * b[k*3+j];
            return newMat;
        }

        public void mul(Matrix3 b)
        {
            Matrix3 newMat = new Matrix3();
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    newMat[i * 3 + j] = 0;

            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    for (int k = 0; k < 3; k++)
                        newMat[i * 3 + j] += this[i * 3 + k] * b[k * 3 + j];
            for (int i = 0; i < 9; i++)
            {
                this[i] = newMat[i];
            }
        }

    }

public class Matrix4
{
    public float[,] m;

    public Matrix4()
    {
        m = new float[4,4];
    }

    public Matrix4(float m00, float m01, float m02, float m03,
                   float m10, float m11, float m12, float m13,
                   float m20, float m21, float m22, float m23,
                   float m30, float m31, float m32, float m33)
    {
        m = new float[4, 4];
        m[0, 0] = m00; m[0, 1] = m01; m[0, 2] = m02; m[0, 3] = m03;
        m[1, 0] = m10; m[1, 1] = m11; m[1, 2] = m12; m[1, 3] = m13;
        m[2, 0] = m20; m[2, 1] = m21; m[2, 2] = m22; m[2, 3] = m23;
        m[3, 0] = m30; m[3, 1] = m31; m[3, 2] = m32; m[3, 3] = m33;
    }

    public static Matrix4 ang2Mat(float hang, float vang)
    {
        // Create rotation matrix "by hand"
        float ch = (float)Math.Cos(hang);
        float cv = (float)Math.Cos(vang);
        float sh = (float)Math.Sin(hang);
        float sv = (float)Math.Sin(vang);

        float[] rotMat = new float[16];
        rotMat[0] = ch; rotMat[2] = cv * sh;
        rotMat[4] = 0; rotMat[6] = sv;
        rotMat[8] = -sh; rotMat[10] = cv * ch;
        // ydir = zdir cross xdir:
        rotMat[1] = rotMat[6] * rotMat[8] - rotMat[10] * rotMat[4]; // c.x = a.y * b.z - a.z * b.y
        rotMat[5] = rotMat[10] * rotMat[0] - rotMat[2] * rotMat[8]; // c.y = a.z * b.x - a.x * b.z
        rotMat[9] = rotMat[2] * rotMat[4] - rotMat[6] * rotMat[0]; // c.z = a.x * b.y - a.y * b.x

        rotMat[3] = 0.0f;
        rotMat[7] = 0.0f;
        rotMat[11] = 0.0f;
        rotMat[12] = 0.0f; rotMat[13] = 0.0f; rotMat[14] = 0.0f; rotMat[15] = 1.0f;

        return new Matrix4(
          rotMat[0], rotMat[1], rotMat[2], rotMat[3],
          rotMat[4], rotMat[5], rotMat[6], rotMat[7],
          rotMat[8], rotMat[9], rotMat[10], rotMat[11],
          rotMat[12], rotMat[13], rotMat[14], rotMat[15]
        );
    }

    public static Matrix4 mul(Matrix4 a, Matrix4 b)
    {
        Matrix4 newMat = new Matrix4();
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                newMat.m[i, j] = 0;

       for(int i=0; i<4; i++)
           for (int j = 0; j < 4; j++)
               for (int k = 0; k < 4; k++)
                   newMat.m[i, j] += a.m[i, k] * b.m[k, j];
       return newMat;
    }

    public static Matrix4 getMVP()
    {
        Matrix4 projection = GL.getProjection();
        Matrix4 model_view = GL.model_view();
        Matrix4 mvp = Matrix4.mul(model_view, projection);
        return mvp;
    }

    // this method works like a charm
    public static Vector3 glToScreen(Matrix4 mvp, Vector3 v)
    {
        float[] depthRange = new float[2];
        GL.glGetFloatv(GL.GL_DEPTH_RANGE, depthRange);

        // Transform the vertex
        Vector4 mvpVertex = new Vector4();
        Vector4 input = new Vector4(v.x, v.y, v.z, 1.0f);

        for (int i = 0; i < 4; i++)
        {
            mvpVertex[i] = Vector4.Dot(new Vector4(mvp.m[0, i], mvp.m[1, i], mvp.m[2, i], mvp.m[3, i]), input);
        }

        Vector4 ndc = mvpVertex / mvpVertex.w;

        float[] viewport = new float[4];
        GL.glGetFloatv(GL.GL_VIEWPORT, viewport);

        // Homogeneous divide
        float rhw = 1.0f / mvpVertex.w;
        float resultx = (1.0f + mvpVertex.x * rhw) * viewport[2] / 2.0f + viewport[0];
        float resulty = (1.0f - mvpVertex.y * rhw) * viewport[3] / 2.0f + viewport[1];

        float zWindow = (depthRange[1] - depthRange[0]) / 2.0f * ndc.z + (depthRange[1] + depthRange[0]) / 2.0f;
        return new Vector3((float)resultx, (float)resulty, (float)zWindow);

    }

    public Vector3 rotate(Vector3 viewVector)
    {
        return new Vector3(m[0, 0] * viewVector.x + m[0, 1] * viewVector.y + m[0, 2] * viewVector.z,
                           m[1, 0] * viewVector.x + m[1, 1] * viewVector.y + m[1, 2] * viewVector.z,
                           m[2, 0] * viewVector.x + m[2, 1] * viewVector.y + m[2, 2] * viewVector.z);
    }

}



public struct Vector4
{
    public float x, y, z, w;

    public Vector4(float x, float y, float z, float w)
    {
        this.x = x;
        this.y = y;
        this.z = z;
        this.w = w;
    }

    public float this[int i]
    {
        get { 
            if (i == 0) return this.x; 
            if (i == 1) return this.y; 
            if (i == 2) return this.z; 
            if (i == 3) return this.w;
            return 0.0f;
        }
        set {
            if (i == 0) this.x = value;
            if (i == 1) this.y = value;
            if (i == 2) this.z = value;
            if (i == 3) this.w = value; 
        }
    }


    public static float Dot(Vector4 v1, Vector4 v2)
    {
        return
        (
           v1.x * v2.x +
           v1.y * v2.y +
           v1.z * v2.z +
           v1.w * v2.w
        );
    }

    public float dot(Vector4 other)
    {
        return Vector4.Dot(this, other);
    }

    public static Vector4 operator /(Vector4 v1, float s2)
    {
        return
        (
           new Vector4
           (
              v1.x / s2,
              v1.y / s2,
              v1.z / s2,
              v1.w / s2
           )
        );
    }

    public static Vector4 operator /(float s1, Vector4 v2)
    {
        return v2 / s1;
    }



    public static Vector4 glToScreen(Vector4 v)
    {

        // Get the matrices and viewport
        float[] modelView = new float[16];
        float[] projection = new float[16];
        float[] viewport = new float[4];
        float[] depthRange = new float[2];

        GL.glGetFloatv(GL.GL_MODELVIEW_MATRIX, modelView);
        GL.glGetFloatv(GL.GL_PROJECTION_MATRIX, projection);
        GL.glGetFloatv(GL.GL_VIEWPORT, viewport);
        GL.glGetFloatv(GL.GL_DEPTH_RANGE, depthRange);

        // Compose the matrices into a single row-major transformation
        Vector4[] T = new Vector4[4];
        int r, c, i;
        for (r = 0; r < 4; ++r)
        {
            for (c = 0; c < 4; ++c)
            {
                T[r][c] = 0;
                for (i = 0; i < 4; ++i)
                {
                    // OpenGL matrices are column major
                    T[r][c] += projection[r + i * 4] * modelView[i + c * 4];
                }
            }
        }

        // Transform the vertex
        Vector4 result = new Vector4();
        for (r = 0; r < 4; ++r)
        {
            result[r] = T[r].dot(v);
        }

        // Homogeneous divide
        float rhw = 1 / result.w;
        float xp = (1 + result.x * rhw) * viewport[2] / 2 + viewport[0];
        float yp = (1 - result.y * rhw) * viewport[3] / 2 + viewport[1];
        float zWindow = (depthRange[1] - depthRange[0]) / 2.0f * result.z + (depthRange[1] + depthRange[0]) / 2.0f;
        // (result.z * rhw) * (depthRange[1] - depthRange[0]) + depthRange[0]
        return new Vector4(xp, yp, zWindow, rhw);
    }


}

[StructLayout(LayoutKind.Sequential)]
public struct Vector3
{
    //http://www.codeproject.com/KB/recipes/VectorType.aspx
    public float x, y, z;

    public Vector3(float x, float y, float z)
    {
        this.x = x; this.y = y; this.z = z;
    }

    public void set(float x, float y, float z)
    {
        this.x = x; this.y = y; this.z = z;
    }

    public float this[int i]
    {
        get
        {
            if (i == 0) return this.x;
            if (i == 1) return this.y;
            if (i == 2) return this.z;
            return 0.0f;
        }
        set
        {
            if (i == 0) this.x = value;
            if (i == 1) this.y = value;
            if (i == 2) this.z = value;
        }
    }

    public void ZERO()
    {
        x = 0; y = 0; z = 0;
    }

    public static Vector3 operator +(Vector3 v1, Vector3 v2)
    {
        return
        (
           new Vector3
           (
              v1.x + v2.x,
              v1.y + v2.y,
              v1.z + v2.z
           )
        );
    }

    public static Vector3 operator -(Vector3 v1, Vector3 v2)
    {
        return
        (
           new Vector3
           (
               v1.x - v2.x,
               v1.y - v2.y,
               v1.z - v2.z
           )
        );
    }

    // Negation
    public static Vector3 operator -(Vector3 v1)
    {
        return
        (
           new Vector3
           (
              -v1.x,
              -v1.y,
              -v1.z
           )
        );
    }



    public static Vector3 operator *(Vector3 v1, float s2)
    {
        return
        (
           new Vector3
           (
              v1.x * s2,
              v1.y * s2,
              v1.z * s2
           )
        );
    }

    public static Vector3 operator *(float s1, Vector3 v2)
    {
        return v2 * s1;
    }


    public static Vector3 operator *(Vector3 v1, Vector3 v2)
    {
        return
        (
           new Vector3
           (
              v1.x * v2.x,
              v1.y * v2.y,
              v1.z * v2.z
           )
        );
    }



    public static float Dot(Vector3 v1, Vector3 v2)
    {
        return
        (
           v1.x * v2.x +
           v1.y * v2.y +
           v1.z * v2.z
        );
    }

    public static Vector3 CrossProduct(Vector3 v1, Vector3 v2)
    {
        return
        (
           new Vector3
           (
              v1.y * v2.z - v1.z * v2.y,
              v1.z * v2.x - v1.x * v2.z,
              v1.x * v2.y - v1.y * v2.x
           )
        );
    }

    public static Vector3 lerp(Vector3 a, Vector3 b, float t)
    {
        Vector3 ret;
        ret.x = (1.0f - t) * a.x + t * b.x;
        ret.y = (1.0f - t) * a.y + t * b.y;
        ret.z = (1.0f - t) * a.z + t * b.z;
        return ret;
    }

    private static float smoothstep(float x)
    {
        return ((x) * (x) * (3.0f - 2.0f * (x)));
    }

    public static Vector3 smoothstep(Vector3 a, Vector3 b, float t)
    {
        Vector3 ret = new Vector3();
        for (int i = 0; i < 3; i++)
        {
            //float v = smoothstep(smoothstep(smoothstep(t)));
            float v = t*t*t;
          ret[i] = (b[i] * v) + (a[i] * (1 - v));
        }
        return ret;
    }



    public static float distance(Vector3 a, Vector3 b)
    {
        float dx = a.x - b.x;
        float dy = a.y - b.y;
        float dz = a.z - b.z;
        return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
    }

    public void normalize()
    {
        float len = (float)Math.Sqrt(x * x + y * y + z * z);

        if (len > 0.0f)
        {
            x /= len;
            y /= len;
            z /= len;
        }
        else
        {
            throw new Exception("argh. bad vec");
        }
    }

    public float Magnitude
    {
        get
        {
            return (float)Math.Sqrt(x * x + y * y + z * z);
        }
        set
        {
            if (value < 0.0f)
            {
                throw new ArgumentOutOfRangeException("value", value,
                  NEGATIVE_MAGNITUDE);
            }

            if (x == 0.0f && y == 0.0f && z == 0.0f)
            { throw new ArgumentException(ORAGIN_VECTOR_MAGNITUDE, "this"); }

            this = this * (value / Magnitude);
        }
    }

    private const string NEGATIVE_MAGNITUDE =
      "The magnitude of a Vector must be a positive value, (i.e. greater than 0)";

    private const string ORAGIN_VECTOR_MAGNITUDE =
       "Cannot change the magnitude of Vector(0,0,0)";

    public static float distanceSquared(Vector3 a, Vector3 b)
    {
        float dx = a.x - b.x;
        float dy = a.y - b.y;
        float dz = a.z - b.z;
        return (dx * dx + dy * dy + dz * dz);
    }

    public void randomize(float degrees)
    {
        Vector3 up = new Vector3(this.z, this.x, this.y); // using this vector to get a vector that ISNT linearly Dependant
        Vector3 right = CrossProduct(this, up);
        float D2R = 1.0f / 180.0f * 3.14f; // degees to radians
        float randH = degrees * D2R * Util.nrnd();
        float randV = degrees * D2R * Util.nrnd();
        Matrix3 randMat = Matrix3.ang2mat(randH, randV);
        this =  randMat.transform(this);
        //Matrix4 randomizedTransform = Matrix4.ang2Mat(randH, randV);
        //this =  randomizedTransform.rotate(this);
    }

   // rotasjon x:
   // | 1 0  0 |
   // | 0 c -s |
   // | 0 s  c |
    public void rotateX(float rad)
    {
        float c = (float)Math.Cos(rad); float s = (float)Math.Sin(rad);
        float nx = x;
        float ny = y*c + z*s;
        float nz = y*-s + z*c;
        this.set(nx,ny,nz);
    }

   // rotasjon y:
   // | c 0  s |
   // | 0 1  0 |
   // | -s 0 c |
    public void rotateY(float rad)
    {
        float c = (float)Math.Cos(rad); float s = (float)Math.Sin(rad);
        float nx = x*c + z*-s;
        float ny = y;
        float nz = x*s + z*c;
        this.set(nx,ny,nz);
    }
   // rotasjon z:
   // | c -s 0 |
   // | s c  0 |
   // | 0 0  1 |
    public void rotateZ(float rad)
    {
        float c = (float)Math.Cos(rad); float s = (float)Math.Sin(rad);
        float nx = x*c + y*s;
        float ny = x*-s + y*c;
        float nz = z;
        this.set(nx,ny,nz);
    }


}

[StructLayout(LayoutKind.Sequential)]
public struct Vector2
{
    
    public float x, y;

    public Vector2(float x, float y)
    {
        this.x = x; this.y = y;
    }
}


public class Plane
{
   public Vector3 normal = new Vector3(0.0f, 0.0f, 0.0f);
   public Vector3 point = new Vector3(0.0f, 0.0f, 0.0f);
   public float d;

   public void set3Points(Vector3 v1, Vector3 v2, Vector3 v3)
   {
       Vector3 aux1 = v1 - v2;
       Vector3 aux2 = v3 - v2;
       normal = Vector3.CrossProduct(aux2, aux1);
       normal.normalize();
       point = v2;
       d = -Vector3.Dot(normal, point);
    }

    public void setNormalAndPoint(Vector3 n, Vector3 p)
    {
       normal = n;
       point = p;
       d = -Vector3.Dot(normal,point);
    }

    public float distanceTo(Vector3 testPoint)
    {
        return d + Vector3.Dot(normal, testPoint);
    }
} // end plane class

// spinor class adapted from
// http://www.paradeofrain.com/2009/07/interpolating-2d-rotations/
public class Spinor
{
    public float real;
    public float complex;
   
    public Spinor(float angle)
    {
        real = (float) Math.Cos(angle);
        complex = (float)Math.Sin(angle);
    }
   
    public Spinor(float real, float complex)
    {
        this.real = real;
        this.complex = complex;
    }

   
    public Spinor GetScale(float t)
    {
        return new Spinor(real * t, complex * t);
    }
    public Spinor GetInvert()
    {
        Spinor s = new Spinor(real, -complex);
        return s.GetScale(s.GetLengthSquared());
    }   
    public Spinor GetAdd(Spinor other)
    {
        return new Spinor(real + other.real, complex + other.complex);
    }
    public float GetLength()
    {
        return (float) Math.Sqrt(real * real + complex * complex);
    } 
    public float GetLengthSquared()
    {
        return (real * real + complex * complex);
    }  
    public Spinor GetMultiply(Spinor other)
    {
        return new Spinor(real * other.real - complex * other.complex, real * other.complex + complex * other.real);
    } 
    public Spinor GetNormalized()
    {
        float length = GetLength();
        return new Spinor(real / length, complex / length);
    } 
    public float GetAngle()
    {
        return (float) Math.Atan2(complex, real) * 2;
    }  
    public Spinor Lerp(Spinor startVal, Spinor endVal, float t)
    {
        return startVal.GetScale(1 - t).GetAdd(endVal.GetScale(t)).GetNormalized();
    }  
   
   
    public static Spinor Slerp(Spinor from, Spinor dest, float t)
    {
        float tr;
        float tc;
        float omega, cosom, sinom, scale0, scale1;
       
        //calc cosine
        cosom = from.real * dest.real + from.complex * dest.complex;
       
        //adjust signs
        if (cosom <0){
            cosom = -cosom;
            tc = -dest.complex;
            tr = -dest.real;
        }else{
            tc = dest.complex;
            tr = dest.real;
        }
       
        //coefficients
        if ((1 - cosom)> 0.001 ){ //threshold, use linear interp if too close
            omega = (float)Math.Acos(cosom);
            sinom = (float)Math.Sin(omega);
            scale0 = (float)Math.Sin((1 - t) * omega) / sinom;
            scale1 = (float)Math.Sin(t * omega) / sinom;
        }
        else{
            scale0 = 1 - t;
            scale1 = t;
        }
       
        //calc final
        Spinor res = new Spinor(0, 0);
        res.complex = scale0 * from.complex + scale1 * tc;
        res.real = scale0 * from.real + scale1 * tr;
        return res;
    }
} // end class Spinor

    public class Util
    {
        private static Random randGen = new Random();
        public static float nrnd()
        {
            return (float)(randGen.NextDouble() * 2)-1.0f;
        }
    }


} // eof namespace
